#ifndef     __MOTOR_CTRL_H
#define     __MOTOR_CTRL_H

#include "./prog.h"
#include "./error.h"

namespace utils {

template <typename Motor, uint32_t Duration, int32_t Min, int32_t Max, uint32_t TimeoutDuration>
class MotorCtrl {
public:
    void set_param(float p, float i, float d) {
        m_kp = p;
        m_ki = i;
        m_kd = d;
    }

    void init_ctrl(uint32_t sp) {
        m_target_speed = sp;
        m_speed = 0;
        m_last_err = 0;
        m_sum_err = 0;
        m_last_position = 0;
        m_lock_count = 0;
    }

    bool is_locked() const {
        return  m_lock_count >= 5;
    }

    bool is_timeout() const {
        return m_time_prog.is_after(TimeoutDuration);
    }

    uint8_t is_ok() const {
        if (is_locked()) {
            return ec::Locked;
        }
        if (is_timeout()) {
            return ec::Timeout;
        }
        return ec::Ok;
    }

    void motor_run() {
        set_voltage(Min);
        m_time_prog.reset();
    }

    uint16_t get_speed() const {
        return m_speed;
    }

    void exec(uint32_t position) {
        if (!m_sp_prog.is_after(Duration)) {
            return;
        }
        m_sp_prog.reset();
        m_speed = (position - m_last_position) * 1000 / Duration;
        m_last_position = position;
        if (m_speed <= 2) {
            m_lock_count ++;
        } else {
            m_lock_count = 0;
        }
        exec_pid();
    }

private:
    void exec_pid() {
        int32_t err = ((int32_t) m_target_speed) - ((int32_t) m_speed);
        m_sum_err += err;
        int32_t output = Min 
            + err * m_kp +
            m_sum_err * m_ki +
            (err - m_last_err) * m_kd;
        m_last_err = err;
        set_voltage(output);
    }

    void set_voltage(int32_t output) {
        if (output > Max) {
            output = Max;
        }
        if (output < Min) {
            output = Min;
        }
        static_cast<Motor *>(this)->set_voltage(output);
    }

    prog_t m_sp_prog;
 //   prog_t m_pid_prog;
    prog_t m_time_prog;
    uint32_t m_last_position;
    uint32_t m_speed;
    uint32_t m_target_speed;

    float m_kp;
    float m_ki;
    float m_kd;
    int32_t m_last_err;
    int32_t m_sum_err;

    uint32_t m_lock_count;
};

};


#endif
